package bojan.robots.motion;

import java.util.HashSet;
import java.util.Set;

import bojan.robots.simulation.Obstacle;

import sim.field.continuous.Continuous2D;
import sim.util.Double2D;
import src.bojan.TerrainMap;

public class MotionValidator {

	Continuous2D grid;
	private TerrainMap map;

	public MotionValidator(Continuous2D grid, TerrainMap map) {
		super();
		this.grid = grid;
		this.map = map;
	}

	public Double2D correct(Double2D position) {
		if (map.isObstacle(new Double2D(position.x + 0.499999, position.y)) 
				|| map.isObstacle(new Double2D(position.x - 0.499999, position.y))) {
			position = new Double2D((int)position.x + .5, position.y);
		}
		if (map.isObstacle(new Double2D(position.x , position.y+ 0.499999)) 
				|| map.isObstacle(new Double2D(position.x, position.y - 0.499999))) {
			position = new Double2D(position.x , (int)position.y + .5);
		}

		return position;
	}

}
